#include "Arduino.h"
#include "filter.h"

void filter(short* y,short* yout,int samples){
	int mean_y;
	int i,j,k;
	float dbuffer[3];
	static const float dv0[3] = { 1.0f, -0.98429f, 0.01571f};

	mean_y=0;
	for(i=0;i<samples;i++){
		mean_y+=y[i];
	}
	mean_y/=samples;

	for(i=0;i<samples;i++){
		y[i]-=mean_y;
	}

	for (k = 0; k < 3; k++) {
	  dbuffer[k] = 0;
	}

	for (j = 0; j + 1 <= samples; j++) {
		for (k = 0; k < 2; k++) {
		  dbuffer[k] = dbuffer[k + 1];
		}

		dbuffer[2] = 0.0;
		for (k = 0; k < 3; k++) {
		  dbuffer[k] = dbuffer[k] + y[j] * (0.49215f + -0.49215f * k);
		}

		for (k = 0; k < 2; k++) {
		  dbuffer[k + 1] -= dbuffer[0] * dv0[k + 1];
		}

		yout[j] = (short)dbuffer[0];
  }

}